
#include "Assembly.h"
#include "ArbitraryQuadrature.h"
#include "libmesh/string_to_enum.h"
#include "libmesh/fe_interface.h"
#include "libmesh/periodic_boundary.h"
#include "PeriodicBC.h"
#include "Attitude.h"
#include "Eigen/Geometry"
#include "MooseMesh.h"
using namespace Eigen;

template<>
InputParameters validParams<PeriodicBC>()
{
	InputParameters params = validParams<CFDBC>();
	params.addParam<bool>("auto_translate", false, "auto translate");
	params.addParam<RealVectorValue>("translate", " translate vector");

	return params;
}

PeriodicBC::PeriodicBC(const InputParameters & parameters):
	CFDBC(parameters),
	_serialized_solution(_sys.serializedSolution()),
	_dof_map(_sys.dofMap()),
	_dim(_mesh.dimension()),
	_fe_type(_var.feType()),
	_fe(FEBase::build(_dim, _fe_type).release()),
	_qrule_arbitrary(new ArbitraryQuadrature(_dim)),
	_phi_periodic(_fe->get_phi())
{
	_fe->attach_quadrature_rule(_qrule_arbitrary);

	std::vector<RealVectorValue> translate(3);
	RealVectorValue bound;
	for (unsigned int i=0; i<_dim; ++i)
	{
		bound(i) =  (_mesh.dimensionWidth(i));
	}
	_translate[0] = Point(bound(0), 0, 0);
	_translate[1] = Point(0, bound(1), 0);
	_translate[2] = Point(0, 0, bound(2));
}

void PeriodicBC::precalculateJacobian()
{
	precalculateResidual();
	for (int q = 0; q < _n_equation; ++q)
	{
		_flux_old[q] = _flux[q];
		_lift_old[q] = _lift[q];
	}

	for (int q = 0; q < _n_equation; ++q)
	{
		_cfd_data.uh[q] += _perturbation;
		reinitFixBC();
		for (int p = 0; p < _n_equation; ++p)
		{
			_flux_jacobi_variable[p][q] = (_flux[p] - _flux_old[p])/_perturbation;
			_lift_jacobi_variable[p][q] = (_lift[p] - _lift_old[p])/_perturbation;
		}
		_cfd_data.uh[q] -= _perturbation;
	}

	for (int q = 0; q < _n_equation; ++q)
	for (int beta = 0; beta < 3; ++beta)
	{
		_cfd_data.duh[q](beta) += _perturbation;
		reinitFixBC();
		for (int p = 0; p < _n_equation; ++p)
		{
			_flux_jacobi_grad_variable[p][q](beta) = (_flux[p] - _flux_old[p])/_perturbation;
		}
		_cfd_data.duh[q](beta) -= _perturbation;
	}
}

void PeriodicBC::boundaryCondition()
{
	Point p1 = periodicPoint(_q_point[_qp]);
	UniquePtr<PointLocatorBase> locate = _mesh.getMesh().sub_point_locator();
	// locate->enable_out_of_mesh_mode();

	const Elem * elem = (*locate)(p1);
    if(!elem)
    {
    	std::cout << "_curr_elem->id(): " << _current_elem->id() <<std::endl;
    	mooseError("no elem");
    }

    std::vector<Point> point_vec(1, p1);
    std::vector<Point> reference_points;
    if(elem->is_linear())
    	FEInterface::inverse_map(elem->dim(), FEType(FIRST), elem, point_vec, reference_points);
    else
    	FEInterface::inverse_map(elem->dim(), FEType(SECOND), elem, point_vec, reference_points);

    _qrule_arbitrary->setPoints(reference_points);
    _fe->reinit(elem);
    for(int eq = 0; eq != _n_equation; ++eq)
    {
    	_dof_map.dof_indices(elem, _dof_indices, eq);
    	unsigned int num_dofs = _dof_indices.size();
    	dof_id_type idx = 0;
    	int qp = 0;
    	Real u(0);
    	for (unsigned int i=0; i < num_dofs; i++)
    	{
    		idx = _dof_indices[i];
    		u += _phi_periodic[i][qp] * _serialized_solution(idx);
    	}
    	_cfd_data_neighbor.uh[eq] = u;
    }
}

Real PeriodicBC::computeQpJacobian(unsigned int p, unsigned int q)
{
	Real r(0);
	r = _flux_jacobi_variable[p][q]*_phi[_j][_qp]*_test[_i][_qp];
	r += _flux_jacobi_grad_variable[p][q]*_grad_phi[_j][_qp]*_test[_i][_qp];
	r += _lift_jacobi_variable[p][q]*_grad_test[_i][_qp]*_phi[_j][_qp];

	return r;
}

void PeriodicBC::reinitFixBC()
{
	_cfd_data.reinit();
	fluxRiemann();
	liftOperator();
}

Point PeriodicBC::periodicPoint(const Point& p)
{
	Point p1;
	if(getParam<bool>("auto_translate"))
	{
		Point p1;
		if(_dim == 1)
		{
			if(_current_boundary_id == 0)
				p1 = _q_point[_qp] + _translate[0];
			else if(_current_boundary_id == 1)
				p1 = _q_point[_qp] - _translate[0];
			else mooseError("invalid boundary: ", _current_boundary_id);

		}
		if(_dim == 2)
		{
			if(_current_boundary_id == 0)
				p1 = _q_point[_qp] + _translate[1];
			else if(_current_boundary_id == 1)
				p1 = _q_point[_qp] - _translate[0];
			else if(_current_boundary_id == 2)
				p1 = _q_point[_qp] - _translate[1];
			else if(_current_boundary_id == 3)
				p1 = _q_point[_qp] + _translate[0];
			else mooseError("invalid boundary: ", _current_boundary_id);
		}

		if(_dim == 3)
		{
			if     (_current_boundary_id == 0)
				p1 = _q_point[_qp] + _translate[2];
			else if(_current_boundary_id == 1)
				p1 = _q_point[_qp] + _translate[1];
			else if(_current_boundary_id == 2)
				p1 = _q_point[_qp] - _translate[0];
			else if(_current_boundary_id == 3)
				p1 = _q_point[_qp] - _translate[1];
			else if(_current_boundary_id == 4)
				p1 = _q_point[_qp] + _translate[0];
			else if(_current_boundary_id == 5)
				p1 = _q_point[_qp] - _translate[2];
			else mooseError("invalid boundary: ", _current_boundary_id);
		}

		return p1;
	}

	return p + getParam<RealVectorValue>("translate");
}
